import ctypes
import logging
import os
import time
import socket
import math
import serial

class POSE(ctypes.Structure):
    _fields_ = [("px", ctypes.c_float),
                ("py", ctypes.c_float),
                ("pz", ctypes.c_float),
                ("rx", ctypes.c_float),
                ("ry", ctypes.c_float),
                ("rz", ctypes.c_float)]

class Arm():

    def __init__(self,code,host):


        CUR_PATH = os.path.dirname(os.path.realpath(__file__))
        dllPath = os.path.join(CUR_PATH, "RM_Base.dll")
        self.pDll = ctypes.cdll.LoadLibrary(dllPath)

        self.pDll.RM_API_Init(code, 0)

        logging.info('开始进行机械臂API初始化完毕')

        #连接机械臂
        byteIP = bytes(host, "gbk")
        self.nSocket = self.pDll.Arm_Socket_Start(byteIP, 8080, 200)


        state = self.pDll.Arm_Socket_State(self.nSocket)

        if state:
            logging.info(f'连接机械臂连接失败:{state}')

        else:

            logging.info(f'连接机械臂成功:{self.nSocket}')


        self.init_first()

    def init_first(self):
        #设置机械臂末端参数为初始值
        nRet = self.pDll.Set_Arm_Tip_Init(self.nSocket, 1)

        logging.info(f'设置机械臂末端参数为初始值')



    def horizontal(self,pose,v,r,block):
        """
        直线运动
        
       Movel_Cmd 笛卡尔空间直线运动
       pose 目标位姿,位置单位：米，姿态单位：弧度
       v 速度比例1~100，即规划速度和加速度占机械臂末端最大线速度和线加速度的百分比
       r 轨迹交融半径，目前默认0。
        block 1 阻塞 0 非阻塞
       return:0-成功，失败返回:错误码, rm_define.h查询
        """
        po1 = POSE()
        po1.px,po1.py,po1.pz,po1.rx,po1.ry,po1.rz = pose[0],pose[1],pose[2],pose[3],pose[4],pose[5]
        self.pDll.Movel_Cmd.argtypes = (ctypes.c_int, POSE, ctypes.c_byte, ctypes.c_float, ctypes.c_int)
        self.pDll.Movel_Cmd.restype = ctypes.c_int
        tag = self.pDll.Movel_Cmd(self.nSocket,po1,v,r,block)
        logging.info(f'Movel_Cmd:{tag}')


        return tag

    def z_run(self,type_,step,v,block):
        """
        Pos_Step_Cmd 当前工作坐标系下，位置步进
        ArmSocket socket句柄
        type 示教类型 x:0 y:1 z:2
        step 步进的距离，单位m，精确到0.001mm
        v 速度比例1~100，即规划速度和加速度占机械臂末端最大线速度和线加速度的百分比
        block RM_NONBLOCK-非阻塞，发送后立即返回; RM_BLOCK-阻塞，等待机械臂返回失败或者到达位置指令
        return 0-成功，失败返回:错误码, rm_define.h查询.
        """

        tag = self.pDll.Pos_Step_Cmd(self.nSocket,type_,step,v,block)
        logging.info(f'Pos_Step_Cmd:{tag}')
        return tag


    def updown(self,joint,v,r,block):
        """
       Movej_Cmd 关节空间运动
       ArmSocket socket句柄
       joint 目标关节1~7角度数组
       v 速度比例1~100，即规划速度和加速度占关节最大线转速和加速度的百分比
       r 轨迹交融半径，目前默认0。
        block 1 阻塞 0 非阻塞
       return 0-成功，失败返回:错误码, rm_define.h查询.
       :return:
        """

        float_joint = ctypes.c_float * 6
        joint = float_joint(*joint)

        self.pDll.Movej_Cmd.argtypes = (ctypes.c_int, ctypes.c_float * 6, ctypes.c_byte,
                                   ctypes.c_float, ctypes.c_bool)

        self.pDll.Movej_Cmd.restype = ctypes.c_int

        tag = self.pDll.Movej_Cmd(self.nSocket,joint,v,r,block)
        logging.info(f'Movej_Cmd:{tag}')
        return tag



    def clamping(self,speed,force,block):

        """
        Set_Gripper_Pick_On 手爪力控夹取
        ArmSocket socket句柄
        speed 手爪夹取速度 ，范围 1~1000，无单位量纲 无
        force 力控阈值 ，范围 ：50~1000，无单位量纲 无
        block 1 阻塞 0 非阻塞
        return 0-成功，失败返回:错误码, rm_define.h查询.
        """

        tag = self.pDll.Set_Gripper_Pick(self.nSocket,speed,force,block)
        logging.info(f'Set_Gripper_Pick:{tag}')

        return tag



    def release(self,speed,block):
        
        """
        Set_Gripper_Release 手爪松开
        ArmSocket socket句柄
        speed 手爪松开速度 ，范围 1~1000，无单位量纲
        block 1 阻塞 0 非阻塞
        return 0-成功，失败返回:错误码

        """

        tag = self.pDll.Set_Gripper_Release(self.nSocket,speed,block)
        logging.info(f'Set_Gripper_Release执行结果{tag}')
        return tag

    def chassis_movement(self,height,speed,block):
        """
        ArmSocket socket句柄
        height  目标高度，单位mm，范围：0~2600
        speed  速度百分比，1~100
        block 1 阻塞 0 非阻塞
        :return:0-成功，失败返回:错误码
        """
        tag = self.pDll.Set_Lift_Height(self.nSocket,height,speed,block)
        logging.info(f'Set_Lift_Height执行结果：{tag}')

        return tag

    def central_rail(self,io,num,state,block):
        """
        中央导轨移动、装备工具、木块推送以及吸笔吸收木块都使用这个命令
        Set_IO_State 设置数字IO状态
        ArmSocket socket句柄
        IO 0-数字IO  1-模拟IO  0
        num 通道号，1~4        1
        state true-高，   false-低 #通过 高 低来控制方向
        block 1 阻塞 0 非阻塞 阻塞
        return 0-成功，失败返回:错误码
        """
        tag = self.pDll.Set_IO_State(self.nSocket,io,num,state,block)
        logging.info(f'Set_IO_State执行的结果：{tag}')

        return tag


    def open_draw(self,type,block):
        """
        打开夹抓 设置工具端电压输出
        param ArmSocket socket句柄
        type 电源输出类型，0-0V，1-5V，2-12V，3-24V
        block 1 阻塞 0 非阻塞
        return 0-成功，失败返回:错误码
        :return: 
        """


        tag = self.pDll.Set_Tool_Voltage(self.nSocket,type,block)
        logging.info(f'设置工作端电压输出结果：{tag}')
        return tag


    def coor_motion(self,pose,v,r,block):
        """
        该函数用于关节空间运动到目标位姿
        param ArmSocket socket句柄
        pose: 目标位姿，位置单位：米，姿态单位：弧度。注意：该目标位姿必须是机械臂末端末端法兰中心基于基坐标系的位姿！！
        v: 速度比例1~100，即规划速度和加速度占机械臂末端最大线速度和线加速度的百分比
        r: 轨迹交融半径，目前默认0。
        block 1 阻塞 0 非阻塞
        return 0-成功，失败返回:错误码

        """
        tag = self.pDll.Movej_P_Cmd(self.nSocket,pose,v,r,block) #pose 相机识别计算结果  v:20 r:0 block:1
        logging.info(f'Movej_P_Cmd执行结果:{tag}')
        return tag


    def io_judgment(self,label):
        """
        判断中央导轨是否到达了两端传感器 如果接近
        
        
        Get_IO_Input 查询所有数字和模拟IO的输入状态
        ArmSocket socket句柄
        DI_state 数字IO输入通道1~3状态数组地址，1-高，0-低
        AI_voltage 模拟IO输入通道1~4输入电压数组
        return 0-成功，失败返回:错误码, rm_define.h查询.
        """


        DI_state = (ctypes.c_byte * 3)(1, 1, 1)  # 假设 DI_state 的长度为 3，初始值为 1
        AI_voltage = (ctypes.c_float * 4)(1.0, 1.0, 1.0, 1.0)  # 假设 AI_voltage 的长度为 4，初始值为 1.0

        logging.info(f'DI_state:{DI_state[0]}')
        print(f'DI_state:{DI_state[0]}')


        if label:
            while DI_state[0] == 1:
                time.sleep(0.5)
                ret = self.pDll.Get_IO_Input(self.nSocket, DI_state, AI_voltage)
                print('DI_state',DI_state[0])
                logging.info(f'Get_IO_Input:{ret}')
                print(f'Get_IO_Input:{ret}')


                if ret == 0 and DI_state[0] == 0:
                    print(f"Get_IO_Input 到达地方:{ret}")
                    return
                elif ret != 0:
                    print(f"Get_IO_Input 调用失败，返回值为 {ret}")

        else:
            print('label',label)
            while DI_state[2] == 1:   #如果是中央导轨从右往左移动  左机械臂 3通道获取 中央导轨的输入信号  ，操作中央导轨都是右机械臂
                time.sleep(0.5)
                ret = self.pDll.Get_IO_Input(self.nSocket, DI_state, AI_voltage)
                print('DI_state', DI_state[2])
                logging.info(f'Get_IO_Input:{ret}')
                print(f'Get_IO_Input:{ret}')


                if ret == 0 and DI_state[2] == 0:
                    print(f"Get_IO_Input 到达地方:{ret}")
                    return
                elif ret != 0:
                    print(f"Get_IO_Input 调用失败，返回值为 {ret}")


    def is_suo(self):
        """
        IO信号
        判断推杆是否缩回

        Get_IO_Input 查询所有数字和模拟IO的输入状态
        ArmSocket socket句柄
        DI_state 数字IO输入通道1~3状态数组地址，1-高，0-低
        AI_voltage 模拟IO输入通道1~4输入电压数组
        return 0-成功，失败返回:错误码, rm_define.h查询.
        """

        DI_state = (ctypes.c_byte * 3)(0, 0, 0)  # 假设 DI_state 的长度为 3，初始值为 0
        AI_voltage = (ctypes.c_float * 4)(1.0, 1.0, 1.0, 1.0)  # 假设 AI_voltage 的长度为 4，初始值为 1.0

        logging.info(f'DI_state:{DI_state[2]}')
        print(f'DI_state:{DI_state[2]}')

        while DI_state[2] == 0:
            time.sleep(0.5)
            ret = self.pDll.Get_IO_Input(self.nSocket, DI_state, AI_voltage)
            print('DI_state', DI_state[0])
            logging.info(f'Get_IO_Input:{ret}')
            print(f'Get_IO_Input:{ret}')

            if ret == 0 and DI_state[2] == 1:
                print(f"Get_IO_Input 到达地方:{ret}")
                return
            elif ret != 0:
                print(f"Get_IO_Input 调用失败，返回值为 {ret}")



class SerCom():
    """
    串口建立，发送 接受
    """


    def __init__(self,path,baud_rate):

        #串口设备的名称，例如 /dev/ttyUSB0（Linux 系统）或 COM1（Windows 系统）
        #波特率，即数据传输速率


        try:
            # 打开串口
            self.ser = serial.Serial(path, baud_rate, timeout=1, parity=serial.PARITY_NONE, bytesize=serial.EIGHTBITS,
                                     stopbits=serial.STOPBITS_ONE)
        except serial.SerialException as e:
            # 捕获异常并打印错误原因
            logging.info('串口连接失败！原因为：', e)
        else:
            # 连接成功，打印串口信息
            print('串口连接成功！串口信息：', self.ser)

    def is_connect(self):
        """
        判断ser是否连接成功
        """

        if self.ser.is_open:
            print('串口连接成功！')
            logging.info('串口建立连接')
        else:
            logging.info('串口连接失败')


    def send(self):

        data = bytes.fromhex('01 06 60 02 00 10 37 C6')
        # 发送数据
        self.ser.write(data)

    def read(self):

        ret = self.ser.read()

    def close(self):

        self.ser.close()

class SockCom():



    """
    sock的建立 发送 接受
    """

    def __init__(self,ip,port):

        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.setblocking(True)  #设置阻塞
        # 连接到目标主机和端口
        server_address = (ip, port)
        logging.info('connecting to %s port %s' % server_address)
        self.tag = self.sock.connect_ex(server_address)

        self.is_connect()

    def is_connect(self):

        if self.tag: #非0显示连接失败
            logging.error(f'socket连接失败:{self.tag}')

        else:
            logging.info(f'socket连接成功')



    def send(self,character):


        """
        character:给照相机发送的指令 发送指令 paizhao就会拍照
        """

        self.message = character
        logging.info('sending "%s"' % self.message)
        self.sock.sendall(self.message.encode())

    def recv(self):
        """

        :return: material,px,py  代表当前识别出的颜色的数字，物品在相机坐标系x轴的位置，物品在相机坐标系y轴的位置

        """

        # 接收返回值
        logging.info('开始拍照')
        data = self.sock.recv(1024)
        str2 = data.decode()



        while str2 == "00":
            logging.info(f'当前相机返回的信息：00')

            self.sock.send(self.message.encode('utf-8'))
            data = self.sock.recv(1024)
            str2 = data.decode()
            time.sleep(2)

        logging.info(f'当前相机返回的信息：{str2}')

        try:
            result = str2.split(',')

            if len(result) > 1:
                material = result[0]
                px = float(result[1])/1000 - 0.26198 #吸取基准点X，示教点为-0.26116
                py = float(result[2])/1000 + 0.14836 #吸取基准点Y，示教点为0.14273

                # px = float(result[1])/1000 - 0.26
                # py = float(result[2])/1000 + 0.15



                rz = float(result[3])* math.pi /180

                return [material, px, py, rz]

            else:

                return result[0]

        except Exception as e:
            logging.error(f'拍照处理结果出错：{e}')


    def close(self):

        self.sock.close()








